/**********************************************************************\
*                                                                      *
* Rotate two AX12 from min to max position every five seconds.         *
* The responses from the AX12 are displayed but not processed.         *
* Caution: The noise scares cats.                                      *
*                                                                      *
* Copyright 2012 Keith Marsh                                           *
*                                                                      *
* Licensed under the Apache License, Version 2.0 (the "License");      *
* you may not use this file except in compliance with the License.     *
* You may obtain a copy of the License at                              *
*       http://www.apache.org/licenses/LICENSE-2.0                     *
* Unless required by applicable law or agreed to in writing, software  *
* distributed under the License is distributed on an "AS IS" BASIS,    *
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or      *
* implied.  See the License for the specific language governing        *
* permissions and limitations under the License.                       *
*                                                                      *
\**********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sched.h>
#include <unistd.h>

/********************************************\
*  Preprocessor Macros and Definitions       *
\********************************************/

#define BCM2708_BASE	0x20000000 			// 0x7e000000
#define GPIO_BASE	(BCM2708_BASE + 0x200000)	// 0x7e200000
#define TIMER_BASE	(BCM2708_BASE + 0x003000)	// 0x7e003000
#define AUX_BASE	(BCM2708_BASE + 0x215000)	// 0x7e215000

// GPIO setup macros. 
// Always use INP_GPIO(x) before using OUT_GPIO(x) or SET_GPIO_ALT(x,y)
#define INP_GPIO(g) *(mGPIO+((g)/10)) &= ~(7<<(((g)%10)*3))
#define OUT_GPIO(g) *(mGPIO+((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(mGPIO+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))

/********************************************\
*  Constants                                 *
\********************************************/

#define PAGE_SIZE	0x1000 	// 4K
#define BLOCK_SIZE	0x1000	// 4K

enum eAuxWordOffsets {
  kAuxIRQ		= 0x00,	// AUX_IRQ
  kAuxEnables		= 0x01, // AUX_ENABLES
  kAuxMiniUartIO	= 0x10, // AUX_MU_IO_REG
  kAuxMiniUartIntEnable = 0x11, // AUX_MU_IER_REG
  kAuxMiniUartIntId	= 0x12, // AUX_MU_IIR_REG
  kAuxMiniUartLineCtrl	= 0x13, // AUX_MU_LCR_REG
  kAuxMiniUartModemCtrl = 0x14, // AUX_MU_MCR_REG
  kAuxMiniUartLineStat	= 0x15, // AUX_MU_LSR_REG
  kAuxMiniUartModemStat	= 0x16,
  kAuxMiniUartScratch	= 0x17,
  kAuxMiniUartExtraCtrl	= 0x18,	// AUX_MU_CNTL_REG
  kAuxMiniUartExtraStat	= 0x19, // AUX_MU_STAT_REG
  kAuxMiniUartBaudRate	= 0x1a
};

enum eAuxEnableMask {
  kAuxEnableMiniUart	= 0x01,
  kAuxEnableSPI1	= 0x02,
  kAuxEnableSPI2	= 0x04
};

enum eUART1LineCtrlMask {
  kUART1DLAB		= 0b10000000,
  kUART1Break		= 0b01000000,
  kUART18Bit		= 0b00000011
};

enum eUART1ModemCtrlMask {
  kUART1RTS 		= 0b00000010
};

enum eUART1LineStatMask {
  kUART1XmitIdle	= 0b01000000,
  kUART1XmitEmpty	= 0b00100000,
  kUART1RecvOverrun	= 0b00000010,
  kUART1DataReady	= 0b00000001
};

enum eUART1ModemStatMask {
  kUART1XmitFIFOLevel	= 0x0f000000,
  kUART1RecvFIFOLevel	= 0x000f0000,
  kUART1XmitDone	= 0x00000200,
  kUART1XmitFIFOEmpty	= 0x00000100,
};

enum eUART1ExtraCtrlMask {
  kUART1CTSAssert	= 0b10000000,
  kUART1RTSAssert	= 0b01000000,
  kUART1RTSAuto		= 0b00110000,
  kUART1UseCTS		= 0b00001000,
  kUART1RTSIsRTR	= 0b00000100,
  kUART1TxEnable	= 0b00000010,
  kUART1RxEnable	= 0b00000001
};

enum eGPIOAltFn {
  kGPIOAlt0	= 0b100,
  kGPIOAlt1	= 0b101,
  kGPIOAlt2	= 0b110,
  kGPIOAlt3	= 0b111,
  kGPIOAlt4	= 0b011,
  kGPIOAlt5	= 0b010
};

enum eGPIOPorts {
  kGPIODirectionPort	= 23,
  kGPIODataInPort	= 24,
  kGPIODataOutPort	= 25
};

enum eGPIOWordOffsets {
  kGPIOSetWordOffset	= 7,
  kGPIOClrWordOffset	= 10
};

enum eDataDirection {
  kToPi = 0,
  kFromPi = 1
};

enum eAX12Instructions {
  kPing		= 0x01,
  kReadData	= 0x02,
  kWriteData	= 0x03,
  kRegWrite	= 0x04,
  kAction	= 0x05,
  kReset	= 0x06,
  kSyncWrite	= 0x83
};

enum eAX12Addresses {
  kRAMLED	= 0x19,
  kRAMGoal	= 0x1e,
  kRAMSpeed	= 0x20
};

enum eGPIOPinsUsed {
  kBufferDirectionPin	= 23,
  kScopeTriggerPin	= 18,
  kUART1TxPin		= 14,
  kUART1RxPin		= 15,
  kUART1CTSPin		= 16,
  kUART1RTSPin		= 17
};

enum kTimerWordOffsets {
  kTimerCS	= 0,
  kTimerCLO	= 1,
  kTimerCHI	= 2,
  kTimerC0	= 3,
  kTimerC1	= 4,
  kTimerC2	= 5,
  kTimerC3	= 6
};

enum eBufferDirection {
  kBufferWriting	= 0,
  kBufferReading	= 1
};

enum eStates {
  kStateInvalid,
  kStateUninitialised,
  kStateResetTimer,
  kStateIdle,
  kStateSendAX12,
  kStateSendingAX12,
  kStateSentAX12,
  kStateReceive,
  kStateExit
};

const static int kRecvBuffSize = 40;

/********************************************\
*  Forward Declarations                      *
\********************************************/

void processState ( void );
void changeState ( void );
void printState ( void );
void ax12Command0 ( unsigned char id, enum eAX12Instructions instruction );
void ax12Command1 ( 
     unsigned char id, 
     enum eAX12Instructions instruction, 
     unsigned char addr, 
     unsigned char value );
void ax12Command2 ( 
     unsigned char id, 
     enum eAX12Instructions instruction, 
     unsigned char addr, 
     unsigned char value1,
     unsigned char value2);
void triggerScope ( void );
void gpioBit ( char bit, char val );
void sendByte ( unsigned char byte );
volatile unsigned long *mapMem ( unsigned long armAddr, unsigned int memSize );

/********************************************\
*  Global Variables                          *
\********************************************/

volatile unsigned long *mGPIO;
volatile unsigned long *mAux;
volatile unsigned long *mTimer;
volatile unsigned long *mSetAddr;
volatile unsigned long *mClrAddr;
enum eStates mState = kStateInvalid;
unsigned long mAX12StopClock = 0;
unsigned char mAX12GoalH = 0;
unsigned char mAX12GoalL = 0;
unsigned char *mRecvBuff;
unsigned int mRecvBuffIx = 0;
unsigned long mRecvStopClock;

/********************************************\
*  MAIN                                      *
\********************************************/

int main(int argc, char **argv) {
  mState = kStateUninitialised;
  while ( mState != kStateExit ) {
    processState();
    changeState();
  }
}

/********************************************\
* processState                               *
* Just do the state, don't try and change    *
* the state.                                 *
\********************************************/

void processState() {
  switch ( mState ) {

  case kStateUninitialised:
    mRecvBuff = malloc(kRecvBuffSize);
    initRealTime();
    initGPIO();
    initUART();
    initTimer();
    initAX12();
    break;

  case kStateResetTimer:
    mAX12StopClock = *(mTimer + kTimerCLO) + 5000000;
    break;

  case kStateSendAX12:
    triggerScope();
    gpioBit ( kGPIODirectionPort, kFromPi );
    ax12Command2 ( 1, kWriteData, kRAMGoal, mAX12GoalL, mAX12GoalH );
    ax12Command2 ( 2, kWriteData, kRAMGoal, mAX12GoalL, mAX12GoalH );
    // Switch between 0x03ff and 0x0000.  Sorry for the magic numbers.
    mAX12GoalH = 3 - mAX12GoalH;
    mAX12GoalL = 0xff - mAX12GoalL;
    break;

  case kStateSentAX12:
    gpioBit ( kGPIODirectionPort, kToPi );
    break;

  case kStateReceive:
    mRecvBuff[mRecvBuffIx++] = ( *(mAux + kAuxMiniUartIO) & 0xff );
    break;
  }
}

/********************************************\
* changeState                                *
* Detect if the state needs to change, or    *
* simply transition from one state to        *
* another.  Don't process the state.         *
\********************************************/

void changeState() {
  switch ( mState ) {

  case kStateUninitialised:
    mState = kStateSendAX12;
    break;

  case kStateResetTimer:
    mState = kStateIdle;
    break;

  case kStateSendAX12:
    mState = kStateSendingAX12;
    break;

  case kStateSendingAX12:
    // Finished sending?
    if ( ( *(mAux + kAuxMiniUartLineStat) & kUART1XmitIdle ) ) {
      mState = kStateSentAX12;
    }
    break;

  case kStateSentAX12:
    mState = kStateResetTimer;
    break;

  case kStateIdle:
    // Time to send a comamd?
    if ( *(mTimer + kTimerCLO) > mAX12StopClock ) {
      mState = kStateSendAX12;
    }
    // Is there data to read?
    if ( *(mAux + kAuxMiniUartLineStat) & kUART1DataReady ) {
      mState = kStateReceive;
      mRecvStopClock = *(mTimer + kTimerCLO) + 1000;
    }
    // Are we waiting for receiver timeout?
    if ( mRecvStopClock && ( mRecvStopClock < *(mTimer + kTimerCLO) ) ) {
      int ix = 0;
      while ( ix < mRecvBuffIx ) {
	printf ( "%02x ", mRecvBuff[ix++] );
      }
      putchar ( '\n' );
      mRecvBuffIx = 0;
      mRecvStopClock = 0;
    }
    break;

  case kStateReceive:
    // No more data to receive?
    if ( ( *(mAux + kAuxMiniUartLineStat) & kUART1DataReady ) == 0 ) {
      mState = kStateIdle;
    }
    break;
  }
}

/**********************************************************************\
* initRealTime                                                         *
* Set the process as a real-time process to stop daemons getting the   *
* processor and causing timing problems.  This should lock out system  *
* when running, but it doesn't for me.  TODO Requires investigation.   *
\**********************************************************************/

int initRealTime() {
  struct sched_param param;
  param.sched_priority = 99;
  int status = sched_setscheduler ( 0, SCHED_RR, &param );
  if ( status != 0 ) {
    perror ( "Can't set realtime scheduling policy" );
  }
}

/**********************************************************************\
* initGPIO                                                             *
* May the GPIO memory to the process memory, then Configure the pins   *
* we require and mark the pins for the UART as their ALT config.       *
\**********************************************************************/

int initGPIO() {
  // Map the GPIO and Aux memory
  mGPIO = mapMem ( GPIO_BASE, 0x1000 );

  // UART1 = Mini-UART, UART0 - PL011
  // Set GPIO15 and 16 for UART1 to ALT5 Mini-Uart
  INP_GPIO ( kBufferDirectionPin );
  OUT_GPIO ( kBufferDirectionPin );
  INP_GPIO ( kScopeTriggerPin );
  OUT_GPIO ( kScopeTriggerPin );
  INP_GPIO ( kUART1TxPin );
  SET_GPIO_ALT ( kUART1TxPin, 5 );
  INP_GPIO ( kUART1RxPin );
  SET_GPIO_ALT ( kUART1RxPin, 5 );
  INP_GPIO ( kUART1RTSPin);
  SET_GPIO_ALT ( kUART1RTSPin, 5 );
}
 
/**********************************************************************\
* initUART                                                             *
* Map the AUX memory to the process memory, then configure the UART.   *
\**********************************************************************/

int initUART() {
  mAux = mapMem ( AUX_BASE, 0x1000 );
  // Enable only the mini uart
  *(mAux + kAuxEnables) = kAuxEnableMiniUart;
  // Set Speed
  *(mAux + kAuxMiniUartBaudRate) = 30;
  // 8-bit mode - SEE ERRATA!!
  *(mAux + kAuxMiniUartLineCtrl) = kUART18Bit;
  // Enable Transmitter ONLY and use RTS
  *(mAux + kAuxMiniUartExtraCtrl) = 
    kUART1RTSAuto |
    kUART1TxEnable | 
    kUART1RxEnable;
  // Set RTS
  *(mAux + kAuxMiniUartModemCtrl) = kUART1RTS;
}

/**********************************************************************\
* initUART                                                             *
* Just map the Timer memory to the process memory.                     *
\**********************************************************************/

int initTimer() {
  mTimer = mapMem ( TIMER_BASE, 0x1000 );
}

int initAX12() {
  triggerScope();
  gpioBit ( kGPIODirectionPort, kFromPi );
  ax12Command2 ( 1, kWriteData, kRAMSpeed, 0xff, 0x03 );
  ax12Command2 ( 2, kWriteData, kRAMSpeed, 0xff, 0x03 );
  mAX12GoalH = 3;
  mAX12GoalL = 0xff;
}

/********************************************\
*  ax12Command0                              *
\********************************************/

void ax12Command0 ( unsigned char id, enum eAX12Instructions instruction )
{
  const char length = 2;  
  unsigned char checksum = ~ ( ( id + length + instruction ) & 0xff);

  sendByte ( 0xff );
  sendByte ( 0xff );
  sendByte ( id );
  sendByte ( length );
  sendByte ( instruction );
  sendByte ( checksum );
}

/********************************************\
*  ax12Command1                              *
\********************************************/

void ax12Command1 ( 
     unsigned char id, 
     enum eAX12Instructions instruction, 
     unsigned char addr, 
     unsigned char value )
{
  const char length = 4;  
  unsigned char checksum = ~ ( ( id + length + instruction + addr + value ) & 0xff);
  sendByte ( 0xff );
  sendByte ( 0xff );
  sendByte ( id );
  sendByte ( length );
  sendByte ( instruction );
  sendByte ( addr );
  sendByte ( value );
  sendByte ( checksum );
}

/********************************************\
*  ax12Command2                              *
\********************************************/

void ax12Command2 ( 
     unsigned char id, 
     enum eAX12Instructions instruction, 
     unsigned char addr, 
     unsigned char value1,
     unsigned char value2 )
{
  const char length = 5;  
  unsigned char checksum = ~ ( ( id + length + instruction + addr + value1 + value2 ) & 0xff);
  
  sendByte ( 0xff );
  sendByte ( 0xff );
  sendByte ( id );
  sendByte ( length );
  sendByte ( instruction );
  sendByte ( addr );
  sendByte ( value1 );
  sendByte ( value2 );
  sendByte ( checksum );
}

/*int ax12Status ( void )
{
  gpioBit ( UART_DIRECTION, UART_READ );
  uartExpect ( 0xff );
  uartExpect ( 0xff );
  unsigned char id;
  uartRead ( id );
  unsigned char length;
  uartRead ( length );
  unsigned char error;
  uartRead ( error );
  calcChecksum();
  uartExpect ( checksum );
}
*/

void sendByte ( unsigned char byte )
{
  // Wait till FIFO has only 3 bytes in it
  while ( ( *(mAux + kAuxMiniUartExtraStat) >> 24 ) > 3 )
    ;
  // Write the character
  *(mAux + kAuxMiniUartIO) = byte;
}

void gpioBit ( char bit, char val )
{
  if ( val )
  {
    *(mGPIO + kGPIOSetWordOffset) = ( 1 << bit );
  }
  else
  {
    *(mGPIO + kGPIOClrWordOffset) = ( 1 << bit );
  }
}

void triggerScope ( void )
{
  // Write a pulse for my oscilloscope trigger
  gpioBit ( kScopeTriggerPin, 1 );
  int n = 10; while (n--);
  gpioBit ( kScopeTriggerPin, 0 );
  n = 10; while (n--);
}

/********************************************\
*  Set up a memory to access IO Peripherals  *
\********************************************/

volatile unsigned long *mapMem ( unsigned long armAddr, unsigned int memSize )
{
  char *mem, *map;
  long pageSize = sysconf ( _SC_PAGE_SIZE );

  /* open /dev/mem */
  int memFD = open("/dev/mem", O_RDWR|O_SYNC);
  if ( memFD < 0 ) {
    perror ( "can't open /dev/mem" );
    exit ( 1 );
  }

  // Allocate MAP block
  int status = posix_memalign ( (void **) &mem, pageSize, memSize );
  if ( status != 0 ) 
  {
    perror ( "Can't get aligned memory" );
    exit ( 2 );
  }

  // Now map it
  map = (unsigned char *) mmap (
    (caddr_t) mem,		/* start */
    memSize,			/* length */
    PROT_READ|PROT_WRITE,	/* prot */
    MAP_SHARED|MAP_FIXED,	/* flags */
    memFD,			/* fd */
    armAddr			/* offset */
  );
  if ( (long) map < 0) 
  {
    perror ( "Can't map memory" );
    exit (1);
  }

  close(memFD);

  // Always use volatile pointer!
  return (volatile unsigned long*) map;
}

void printState ( void ) {
  switch ( mState ) {
  case kStateUninitialised:
    printf ( "kUninitialised\n" );
    break;
  case kStateResetTimer:
    printf ( "kResetTimer\n" );
    break;
  case kStateSendAX12:
    printf ( "kSendAX12\n" );
    break;
  case kStateSentAX12:
    printf ( "lSentAX12\n" );
    break;
  case kStateReceive:
    printf ( "kReceive\n" );
    break;
  case kStateIdle:
    printf ( "kIdle\n" );
    break;
  case kStateExit:
    printf ( "kExit\n" );
    break;
  default:
    printf ( "kUnknown\n" );
    break;
  }
}
